#include "ppform.h"
#include "mykroneckerproduct.h"
#include "unsupported/Eigen/KroneckerProduct"
#include <linearsolvers.h>
#include <serializer.h>
#include <iostream>
#include <utilities.h>

namespace SPLINTER
{

    PPForm::PPForm()
        : Function(1)
    {
    }

    PPForm::PPForm(unsigned int numVariables)
        : Function(numVariables)
    {
    }

    /*
     * Constructors for multivariate B-spline using explicit data
     */
    PPForm::PPForm(std::vector<std::vector<double>> knotVectors, std::vector<unsigned int> basisDegrees)
        : Function(knotVectors.size()),
          knotVectors(knotVectors),
          basisDegrees(basisDegrees)
    {
        // Initialize coefficients to ones
        // setPolynomials(DenseVector::Ones());

        checkControlPoints();
    }

    std::vector<Polynomial> vectorToPolynomials(const std::vector<std::vector<double>> &polynomials)
    {
        std::vector<Polynomial> ret;
        for (auto it = polynomials.begin(); it != polynomials.end(); it++)
        {
            ret.push_back(Polynomial(vectorToDenseVector(*it)));
        }
        return ret;
    }

    PPForm::PPForm(std::vector<std::vector<double>> polynomials, std::vector<std::vector<double>> knotVectors, std::vector<unsigned int> basisDegrees)
        : PPForm(vectorToPolynomials(polynomials), knotVectors, basisDegrees)
    {
    }

    PPForm::PPForm(std::vector<Polynomial> polynomials, std::vector<std::vector<double>> knotVectors, std::vector<unsigned int> basisDegrees)
        : Function(knotVectors.size()),
          polynomials(polynomials),
          knotVectors(knotVectors),
          basisDegrees(basisDegrees)
    {
        setPolynomials(polynomials);

        checkControlPoints();
    }

    /*
     * Construct from saved data
     */
    PPForm::PPForm(const char *fileName)
        : PPForm(std::string(fileName))
    {
    }

    PPForm::PPForm(const std::string &fileName)
        : Function(1)
    {
        load(fileName);
    }

    /**
     * Returns the function value at x
     */
    double PPForm::eval(DenseVector x) const
    {
        //checkInput(x);
        // NOTE: casting to DenseVector to allow accessing as res(0)
        double x0 = x(0);
        for (int i = 0; i < knotVectors.size() - 1; i++)
        {
            if (x0 <= knotVectors[i + 1][0])
            {
                double ret = polynomials[i](x0);
                //std::cout << "3333:" <<"x:"<<x0<<",knot:"<<knotVectors[i + 1][0]<<",i:"<< i << ",result:" << ret << std::endl;
                //std::cout<<polynomials[i].to_string().c_str()<<std::endl;
                return ret;
            }
        }
        // DenseVector res = coefficients.transpose() * evalBasis(x);
        // return res(0);
        auto ret = polynomials[knotVectors.size() - 1](x0);

        //std::cout << "3333:" << ret << std::endl;
        return ret;
    }

    /**
     * Returns the (1 x numVariables) Jacobian evaluated at x
     */
    DenseMatrix PPForm::evalJacobian(DenseVector x) const
    {
        checkInput(x);
        return /**coefficients.transpose() *  evalBasisJacobian(x);*/ DenseMatrix();
    }

    /*
     * Returns the Hessian evaluated at x.
     * The Hessian is an n x n matrix,
     * where n is the dimension of x.
     */
    DenseMatrix PPForm::evalHessian(DenseVector x) const
    {
        checkInput(x);

#ifndef NDEBUG
        // if (!pointInDomain(x))
        //     throw Exception("PPForm::evalHessian: Evaluation at point outside domain.");
#endif // NDEBUG

        DenseMatrix H;
        H.setZero(1, 1);
        DenseMatrix identity = DenseMatrix::Identity(numVariables, numVariables);
        DenseMatrix caug; // = kroneckerProduct(identity, coefficients.transpose());
        // DenseMatrix DB = basis.evalBasisHessian(x);
        // H = caug * DB;

        // Fill in upper triangular of Hessian
        for (size_t i = 0; i < numVariables; ++i)
            for (size_t j = i + 1; j < numVariables; ++j)
                H(i, j) = H(j, i);

        return H;
    }

    DenseMatrix PPForm::getControlPoints() const
    {
        int nc = polynomials.size();
        DenseMatrix controlPoints(nc, numVariables + 1);

        controlPoints.block(0, 0, nc, numVariables) = knotaverages;
        // controlPoints.block(0, numVariables, nc, 1) = coefficients;

        return controlPoints;
    }

    void PPForm::setPolynomials(const std::vector<Polynomial> &polynomials)
    {
        /**
         * if (coefficients.size() != getNumBasisFunctions())
            throw Exception("PPForm::setControlPoints: Incompatible size of coefficient vector.");
        */

        this->polynomials = polynomials;
        checkControlPoints();
    }

    void PPForm::setControlPoints(const DenseMatrix &controlPoints)
    {
        /**
        if (controlPoints.cols() != numVariables + 1)
            throw Exception("PPForm::setControlPoints: Incompatible size of control point matrix.");

        int nc = controlPoints.rows();

        knotaverages = controlPoints.block(0, 0, nc, numVariables);
        coefficients = controlPoints.block(0, numVariables, nc, 1);

        checkControlPoints();
        */
    }

    void PPForm::updateControlPoints(const DenseMatrix &A)
    {
        /**
         *
        if (A.cols() != coefficients.rows() || A.cols() != knotaverages.rows())
            throw Exception("PPForm::updateControlPoints: Incompatible size of linear transformation matrix.");
        coefficients = A * coefficients;
        knotaverages = A * knotaverages;
        */
    }

    void PPForm::checkControlPoints() const
    {
        // if (coefficients.rows() != knotaverages.rows())
        //     throw Exception("PPForm::checkControlPoints: Inconsistent size of coefficients and knot averages matrices.");
        // if (knotaverages.cols() != numVariables)
        //    throw Exception("PPForm::checkControlPoints: Inconsistent size of knot averages matrix.");
    }

    void PPForm::save(const std::string &fileName) const
    {
        Serializer s;
        s.serialize(*this);
        s.saveToFile(fileName);
    }

    void PPForm::load(const std::string &fileName)
    {
        Serializer s(fileName);
        s.deserialize(*this);
    }

    std::string PPForm::getDescription() const
    {
        std::string description("PPForm of degree");
        auto degrees = getBasisDegrees();
        // See if all degrees are the same.
        bool equal = true;
        for (size_t i = 1; i < degrees.size(); ++i)
        {
            equal = equal && (degrees.at(i) == degrees.at(i - 1));
        }

        if (equal)
        {
            description.append(" ");
            description.append(std::to_string(degrees.at(0)));
        }
        else
        {
            description.append("s (");
            for (size_t i = 0; i < degrees.size(); ++i)
            {
                description.append(std::to_string(degrees.at(i)));
                if (i + 1 < degrees.size())
                {
                    description.append(", ");
                }
            }
            description.append(")");
        }

        return description;
    }

} // namespace SPLINTER
